#include "inflation_layer.h"

namespace costmap_2d{

InflationLayer::InflationLayer(boost::shared_ptr<Costmap> realtime_costmap_ptr, double inflation_radius)
    :realtimeCostmapPtr_(realtime_costmap_ptr),inflationRadius_(inflation_radius){
    inflationRadius_ = max(0.0, inflationRadius_);
}

InflationLayer::~InflationLayer(){

}

// 计算缓存数据
void InflationLayer::calCacheDate(){
    if(distanceCache_.empty() || costCache_.empty()){
        inflationRadiusPixels_ = ceil(inflationRadius_ / realtimeCostmapPtr_->map_metadata.resolution);
        // cout << "inflationRadiusPixels_ = " << inflationRadiusPixels_ << endl;
        // 计算缓存数据
        int radius_pixels_cache = inflationRadiusPixels_+5;  //防止越界访问
        distanceCache_.resize(radius_pixels_cache);
        costCache_.resize(radius_pixels_cache);
        for(int i=0; i<radius_pixels_cache; ++i){  
            distanceCache_[i].resize(radius_pixels_cache);
            costCache_[i].resize(radius_pixels_cache);
            for(int j=0; j<radius_pixels_cache; ++j){
                double distance = hypot(i, j);
                distanceCache_[i][j] = distance;
    
                if(distance == 0){
                    costCache_[i][j] = costmap_2d::LETHAL_OBSTACLE;
                }
                else{
                    double real_distance = distance * realtimeCostmapPtr_->map_metadata.resolution;
                    // FREE_SPACE - 0
                    // 膨胀区域 1 ~ 253  按照最远MAX_INFLASTION_DISTANCE 2.0米进行计算
                    // LETHAL_OBSTACLE = 254
                    // UNKNOWN - 255 
                    costCache_[i][j] = (costmap_2d::LETHAL_OBSTACLE - 1) * (real_distance / costmap_2d::MAX_INFLASTION_DISTANCE);
                }
    
                // cout << "i: " << i << ", j: " << j 
                //     << ", distance: " << distance 
                //     << ", cost: " << costCache_[i][j]
                //     << endl;
            }
        }  
    }
}

// 更新代价地图
void InflationLayer::updateCostmap(std::unordered_map<int, GridCell>& obstacle_cells){
    // cout << "[InflationLayer::updateCostmap] inflationRadius_: " << inflationRadius_
    //     << ", realtimeCostmapPtr_->costMap.size(): " << realtimeCostmapPtr_->costMap.size()
    //     << ", inflationRadiusPixels_: " << inflationRadiusPixels_  
    //     << endl;
    // 计算缓存数据
    calCacheDate();

    inflation_cells_.clear();
    vector<bool> seen(realtimeCostmapPtr_->costMap.size(), false);

    // 提取所有障碍物数据
    std::vector<GridCell> obs_bin;
    for(auto item : obstacle_cells){
        obs_bin.emplace_back(item.second);
    }
    inflation_cells_[0.0] = obs_bin;

    // cout << "[InflationLayer::updateCostmap] inflationRadius_: " << inflationRadius_
    //     << ", inflationRadiusPixels_: " << inflationRadiusPixels_ 
    //     << ", obstacle_cells size: " << inflation_cells_[0.0].size() 
    //     << endl;

    // 障碍物膨胀
    for (auto iter = inflation_cells_.begin(); iter != inflation_cells_.end(); ++iter){
        for (int i = 0; i < iter->second.size(); ++i){
            const GridCell& cell = iter->second[i];
            int dx = abs(cell.x - cell.sx);
            int dy = abs(cell.y - cell.sy); 
            unsigned char cost = costCache_[dx][dy];

            if(cell.index < 0 || cell.index >= realtimeCostmapPtr_->costMap.size()) {
                continue; // 跳过无效的索引
            }
            if(seen[cell.index]){
                // realtimeCostmapPtr_->costMap[cell.index] = std::max(
                //     realtimeCostmapPtr_->costMap[cell.index],
                //     cost
                // );
                continue;
            }
            seen[cell.index] = true;
            realtimeCostmapPtr_->costMap[cell.index] = cost;
            
            // 添加相邻节点
            GridCell new_cell = cell;
            vector<GridCell> neighbor_cells;
            if(cell.x > 0){     // [x-1, y]
                new_cell.index = cell.index - 1;
                new_cell.x = cell.x - 1;
                new_cell.y = cell.y;
                neighbor_cells.emplace_back(new_cell);
            }
            if(cell.y > 0){     // [x, y-1]
                new_cell.index = cell.index - realtimeCostmapPtr_->map_metadata.width;
                new_cell.x = cell.x;
                new_cell.y = cell.y -1;
                neighbor_cells.emplace_back(new_cell);                
            }
            if(cell.x < realtimeCostmapPtr_->map_metadata.width - 1){    // [x+1, y]
                new_cell.index = cell.index + 1;
                new_cell.x = cell.x + 1;
                new_cell.y = cell.y;
                neighbor_cells.emplace_back(new_cell);                  
            }
            if(cell.y < realtimeCostmapPtr_->map_metadata.height - 1){   // [x, y+1]
                new_cell.index = cell.index + realtimeCostmapPtr_->map_metadata.width;
                new_cell.x = cell.x;
                new_cell.y = cell.y + 1;
                neighbor_cells.emplace_back(new_cell);                  
            }

            for(auto cell_item : neighbor_cells){
                // if(seen[cell.index])
                //     continue;
                int dx_item = abs(cell_item.x - cell_item.sx);
                int dy_item = abs(cell_item.y - cell_item.sy); 
                double distance = distanceCache_[dx_item][dy_item];
                if( distance > inflationRadiusPixels_) 
                    continue;
                inflation_cells_[distance].push_back(cell_item);
            }
        }
    }
}

// 清空代价地图
void InflationLayer::clearCostmap(){
    costCache_.clear();
    distanceCache_.clear();
}

}; //costmap_2d